#include "Camera_Mech.h"
#include "OpenCVUtil.h"

#include "glog/logging.h"

Camera_mech::Camera_mech()
{
    b_isConnect = false;
}

Camera_mech::~Camera_mech()
{
    closeDevice();
}

void Camera_mech::set3D4Ident(bool isOuter)
{
    /*
    exposure:
        Exposure unit: ms
        Exposure minimum: 0.1
        Exposure maximum: 99
        Minimum Vector Size: 1
        Maximum Vector Size: 3
    */

    /*
    enum struct PowerLevel {
        High,   ///< High level is ofen used for scanning dark objects.
        Normal, ///< Normal level is mostly used.
        Low,    /// Low level is used for scanning reflective objects.
    };
    */

    mmind::api::ErrorStatus status;

    if(isOuter)
    {
        m_device.setScan3DExposure(std::vector<double>{20});
        m_device.setProjectorPowerLevel(mmind::api::ProjectorSettings::PowerLevel::Normal);
    }
    else
    {
        m_device.setScan3DExposure(std::vector<double>{8});
        m_device.setProjectorPowerLevel(mmind::api::ProjectorSettings::PowerLevel::High);
    }

    mmind::api::ProjectorSettings::PowerLevel value;
    m_device.getProjectorPowerLevel(value);
    LOG(INFO) << "The 3D scanning Projector PowerLevel : " << (int)value << ".\n";

    std::vector<double> exposureSequence;
    m_device.getScan3DExposure(exposureSequence);

    LOG(INFO) << "The 3D scanning exposure multiplier : " << exposureSequence.size() << ".\n";

    for (size_t i = 0; i < exposureSequence.size(); i++)
        LOG(INFO) << "3D scanning exposure time " << i + 1 << " : " << exposureSequence[i] << " ms.\n";

    //    m_device.saveAllSettingsToUserSets();
}

bool Camera_mech::openDevice()
{
    if (b_isConnect)
    {
        m_device.disconnect();
        b_isConnect = false;
    }

    m_deviceList.clear();
    m_deviceList = mmind::api::MechEyeDevice::enumerateMechEyeDeviceList();

    if (m_deviceList.empty())
    {
        LOG(ERROR) << " No MechDevice Found!!!" << std::endl;
        return false;
    }

    printDeviceInfo();

    m_status = m_device.connect(m_deviceList.at(0));

    if (!m_status.isOK())
    {
        printError();
        return false;
    }

    b_isConnect = true;
    return true;
}

void Camera_mech::closeDevice()
{
    if (b_isConnect)
        m_device.disconnect();

    b_isConnect = false;
    m_deviceList.clear();
}

bool Camera_mech::is_connected()
{
    return b_isConnect;
}

int Camera_mech::transFormatColor(mmind::api::ColorMap& src, cv::Mat& imgColor)
{
    if (src.empty())
    {
        return -1;
    }

    //    LOG(INFO) << src.width() << "*" << src.height() << std::endl;

    cv::Mat color8UC3 = cv::Mat(src.height(), src.width(), CV_8UC3, src.data());
    imgColor = color8UC3.clone();
    return 0;
}

int Camera_mech::transFormatPoint(mmind::api::PointXYZMap& cloud, cv::Mat& imgX, cv::Mat& imgY, cv::Mat& imgZ)
{
    if (cloud.empty())
    {
        return -1;
    }

    int imgHeight = cloud.height();
    int imgWidth = cloud.width();

    imgX = cv::Mat(cv::Size(imgWidth, imgHeight), CV_32FC1, cv::Scalar(0));
    imgY = cv::Mat(cv::Size(imgWidth, imgHeight), CV_32FC1, cv::Scalar(0));
    imgZ = cv::Mat(cv::Size(imgWidth, imgHeight), CV_32FC1, cv::Scalar(0));

    int i = 0;  //row
    int j = 0;  //col
    int PointNum = imgHeight * imgWidth;
    for (size_t k = 0; k < PointNum; ++k)
    {

        if (j == imgWidth)
        {
            ++i;
            if (i == imgHeight)
            {
                break;
            }
            j = 0;
        }
        if (cloud.at(i, j).x != 0)
        {

            imgX.at<float>(i, j) = cloud.at(i, j).x;
            imgY.at<float>(i, j) = cloud.at(i, j).y;
            imgZ.at<float>(i, j) = cloud.at(i, j).z;
        }
        else
        {

            imgX.at<float>(i, j) = 0;
            imgY.at<float>(i, j) = 0;
            imgZ.at<float>(i, j) = 0;
        }
        j++;
    }
    return 0;
}

bool  Camera_mech::capture_color_point(cv::Mat& imgColor,cv::Mat& imgX,cv::Mat& imgY,cv::Mat& imgZ)
{
    mmind::api::ColorMap colorMap;
    if (!m_device.capturePointXYZMap(pointCloud).isOK() || !m_device.captureColorMap(colorMap).isOK() )
    {
        printError();
        return false;
    }
    int ret = transFormatColor(colorMap, imgColor);
    if (ret != 0)
        return false;
    transFormatPoint(pointCloud, imgX, imgY, imgZ);
    if (imgX.empty() || imgY.empty() || imgZ.empty())
        return false;

    return true;
}
bool Camera_mech::capture_pointCloud(cv::Mat& imgX, cv::Mat& imgY, cv::Mat& imgZ)
{
    //    mmind::api::PointXYZMap pointCloud;

    if (!m_device.capturePointXYZMap(pointCloud).isOK())
    {
        printError();
        return false;
    }

    transFormatPoint(pointCloud, imgX, imgY, imgZ);
    if (imgX.empty() || imgY.empty() || imgZ.empty())
        return false;

    return true;
}

bool Camera_mech::capture_colorMap(cv::Mat& imgColor)
{
    mmind::api::ColorMap colorMap;

    if (!m_device.captureColorMap(colorMap).isOK())
    {
        printError();
        return false;
    }
    
    int ret = transFormatColor(colorMap, imgColor);
    if (ret != 0)
        return false;

    return true;
}

bool Camera_mech::capture_depthMap(mmind::api::DepthMap& depthMap)
{
    if (!m_device.captureDepthMap(depthMap).isOK())
    {
        printError();
        return false;
    }
    return true;
}

void Camera_mech::printDeviceInfo()
{
    auto head = m_deviceList.cbegin();
    auto tail = m_deviceList.cend();

    printf("********************3D-Camera******************\n");
    while (head != tail)
    {
        LOG(INFO) << "<Mech Model Name>:" << (*head).model.c_str() << std::endl;
        LOG(INFO) << "<Mech ID>:" << (*head).id.c_str() << std::endl;
        LOG(INFO) << "<Mech IP>:" << (*head).ipAddress.c_str() << "\n" << std::endl;
        head++;
    }
    printf("***********************************************\n");

}

void Camera_mech::printError()
{
    LOG(ERROR) << "Code :" << m_status.errorCode << "  errStr :" << m_status.errorDescription.c_str() << std::endl;
}


int Camera_mech::detect_cloud_rect_xyz(cv::Rect rec, std::vector<float> &reslut)
{
    LOG(INFO) << "Start of detect_cloud_rect_xyz" << std::endl;
    int xcenter = rec.x + (rec.width / 2);
    int ycenter = rec.y + (rec.height / 2);
    /* convertUnit(depth, zed.getInitParameters().coordinate_units, UNIT::MILLIMETER);
                depth.write("D:\\1.png");*/
    //float zcenter = depth_img.ptr<float>(ycenter)[xcenter];
    double z_dis = 0;
    double x_dis = 0;
    double y_dis = 0;
    //    sl::float4 info;
    //    pointCloud.getValue(xcenter, ycenter, &info);
    //    mmind::api::PointXYZMap pointXYZ=pointCloud.at(xcenter, ycenter);

    z_dis = pointCloud.at(ycenter, xcenter).z;
    x_dis = pointCloud.at(ycenter, xcenter).x;
    y_dis = pointCloud.at(ycenter, xcenter).y;

    if (!std::isnormal(z_dis) || z_dis == 0)
    {
        int num = 1;
        while (!std::isnormal(z_dis) || z_dis == 0)
        {
            std::vector<cv::Point3f> points;
            std::vector<float> point_xs;
            std::vector<float> point_ys;
            std::vector<float> point_zs;
            if ( num*5>rec.width/2 || num*5>rec.height/2)
            {
                break;
            }
            for (int i = xcenter-num*5; i < xcenter+num*5 ; i++)
            {
                for (int j = ycenter-num*5; j < ycenter+num*5; j++)
                {
//                    sl::float4 tmpInfo;
//                    point_cloud.getValue(i, j, &tmpInfo);

                    float z = pointCloud.at(j, i).z;
                    float x = pointCloud.at(j, i).x;
                    float y = pointCloud.at(j, i).y;
                    if (!std::isnan(z) && std::isfinite(z) && !std::isinf(z) && std::isnormal(z))
                    {

                        //point_cloud.getValue()
                        cv::Point3f point(x, y, z);

                        /* int *z;
                                     depth.getValue(i, j, z);
                                     cv::Point3f point(i, j, *z)*/;
                        //std::cout << "z=" << z << std::endl;
                        points.push_back(point);
                        point_xs.push_back(x);
                        point_ys.push_back(y);
                        point_zs.push_back(z);
                    }
                }
            }
            //std::cout << "points.size" << points.size() << std::endl;
#ifdef FITTING
            std::vector<double> planeFun;
            if (points.size() > 30)
            {
                CaculatefitPlane(points, planeFun);
            }

            if (planeFun.size() == 3)
            {


                double x_sum = std::accumulate(point_xs.begin(), point_xs.end(), 0);
                double y_sum = std::accumulate(point_ys.begin(), point_ys.end(), 0);

                x_dis = x_sum / point_xs.size();
                y_dis = y_sum / point_ys.size();

                std::cout << "A=" << planeFun[0] << " B=" << planeFun[1] << " c=" << planeFun[2] << std::endl;
                z_dis = planeFun[0] * x_dis + planeFun[1] *y_dis + planeFun[2];
            }
#else
            if(point_zs.size()>30)
            {
                double x_sum = std::accumulate(point_xs.begin(), point_xs.end(), 0);
                double y_sum = std::accumulate(point_ys.begin(), point_ys.end(), 0);
                double z_sum = std::accumulate(point_zs.begin(), point_zs.end(), 0);
                x_dis = x_sum / point_xs.size();
                y_dis = y_sum / point_ys.size();
                z_dis = z_sum / point_zs.size();
            }
#endif
            num++;

        }
    }
    if(z_dis==0){
        LOG(INFO) << "End of detect_cloud_rect_xyz" << std::endl;
        return -1;
    }

    reslut.push_back((float)x_dis);
    reslut.push_back((float)y_dis);
    reslut.push_back((float)z_dis);
    return 0;
}
